#include<SoftwareSerial.h>
//#include<AltSoftSerial.h>

#define LEDPIN 2
#define BUTTONPIN 3
#define TRXRATE 19200
#define TERMBAUD 9600
#define RX_1 6
#define TX_1 7
#define RX_2 8
#define TX_2 9
#define RX_3 10
#define TX_3 11
#define RX_4 12
#define TX_4 13
SoftwareSerial mySerial_1(RX_1,TX_1);
SoftwareSerial mySerial_2(RX_2,TX_2);
SoftwareSerial mySerial_3(RX_3,TX_3);
SoftwareSerial mySerial_4(RX_4,TX_4);

int isFirst;
long rand100;
int collecting_IDs;
uint8_t IDs[17];
uint8_t ID_read[4];
int id_count;
int noNewID = 0;
char msg_buf[16] = "TEST_MESSAGE";
long prevTime;
long time_1;
long time_2;
int ledState = LOW;
uint8_t buf_first;

//function asked a neighbor for its flag value
uint8_t getFlag(int channel) {
	Serial.println("GETTING A FLAG!");
	uint8_t flagValue;
	writebuffertoserial(channel, uint8_t(252), 8) //sends the sendFlag signal
	switch(channel) {
		case 1:
			mySerial_1.listen();
			while(waitForData(1) != uint8_t(0) || waitForData(1) != uint8_t(1)){}; //waits for either a 0 or 1
			flagValue = mySerial_1.read();
			break;
		case 2:
			mySerial_2.listen();
			while(waitForData(2) != uint8_t(0) || waitForData(2) != uint8_t(1)){};
			flagValue = mySerial_2.read();
			break;
		case 3:
			mySerial_3.listen();
			while(waitForData(3) != uint8_t(0) || waitForData(3) != uint8_t(1)){};
			flagValue = mySerial_3.read();
			break;
		case 4:
			mySerial_4.listen();
			while(waitForData(4) != uint8_t(0) || waitForData(4) != uint8_t(1)){};
			flagValue = mySerial_4.read();
			break;
		default:
			Serial.println("Error, invalid channel number in function getFlag!");
			flagValue = uint8_t(-1);
			break;
		}
		writebuffertoserial(channel, uint8_t(249), 8); //sends back the receivedData signal
		return flagValue;
	}
}


//function asks for a neighbors ID
uint8_t getID(int channel) {
	Serial.println("GETTING AN ID!");
	uint8_t idValue;
	writebuffertoserial(channel, uint8_t(251), 8) //sends the sendID signal
	switch(channel) {
		case 1:
			mySerial_1.listen();
			waitForData(1);
			idValue = mySerial_1.read();
			break;
		case 2:
			mySerial_2.listen();
			waitForData(2);
			idValue = mySerial_2.read();
			break;
		case 3:
			mySerial_3.listen();
			waitForData(3);
			idValue = mySerial_3.read();
			break;
		case 4:
			mySerial_4.listen();
			waitForData(4);
			idValue = mySerial_4.read();
			break;
		default:
			Serial.println("Error, invalid channel number in function getID!");
			idValue = uint8_t(-1);
			break;
		}
		writebuffertoserial(channel, uint8_t(249), 8); //sends back the receivedData signal
		return idValue;
	}
}

//function asks for all neighbors IDs
uint8_t* getNeighbor() {
	Serial.println("GETTING NEIGHBORS!");
	uint8_t neighbors[4];
	neighbors[0] = getID(1);
	neighbors[1] = getID(2);
	neighbors[2] = getID(3);
	neighbors[3] = getID(4);
	return neighbors;
}

//function sets the flag to 0 and sends the resetFlag command to its neighbors
void resetFlags() {
	Serial.println("RESETING FLAGS!");
	flag = 0;
	writebuffertoserial(1, uint8_t(250), 8);
	while(waitForData(1) != uint8_t(249)){} //waits for reset conformation
	writebuffertoserial(2, uint8_t(250), 8);
	while(waitForData(2) != uint8_t(249)){}
	writebuffertoserial(3, uint8_t(250), 8);
	while(waitForData(3) != uint8_t(249)){}
	writebuffertoserial(4, uint8_t(250), 8);
	while(waitForData(4) != uint8_t(249)){}
}

//waits for data to be received
uint8_t waitForData(int channel) {
	uint8_t data;
	switch(channel) {
		case 1:
			while(mySerial_1.available() != 8){
				Serial.print("Waiting on data from mySerial_1... ");
			}
			data = mySerial_1.peek();
			Serial.println(data);
			break;
		case 2:
			while(mySerial_2.available() != 8){
				Serial.println("Waiting on data from mySerial_2... ");
			}
			data = mySerial_2.peek();
			Serial.println(data);
			break;
		case 3:
			while(mySerial_3.available() != 8){
				Serial.println("Waiting on data from mySerial_3... ");
			}
			data = mySerial_3.peek();
			Serial.println(data);
			break;
		case 4:
			while(mySerial_4.available() != 8){
				Serial.println("Waiting on data from mySerial_4... ");
			}
			data = mySerial_4.peek();
			Serial.println(data);
			break;
		default:
			Serial.println("Error, invalid channel number in function waitForData!");
			data = uint8_t(-1);
			break;
	}
	return data;
}

void blink_led(int ledPin){
  long blinkTime = 250;
  long curTime = millis();
  if((curTime - prevTime) > blinkTime){
    prevTime = curTime;
    if(ledState == LOW){
      ledState = HIGH;
    }else{
      ledState = LOW;
    }
  }
  digitalWrite(ledPin, ledState);
}

//https://stackoverflow.com/questions/15094834/check-if-a-value-exist-in-a-array
int isvalueinarray(byte val, byte *arr, int size){
    int i;
    for (i=0; i < size; i++) {
        if (arr[i] == val)
            return 1;
    }
    return 0;
}

//Write ID buffer to serial TX
void writebuffertoserial(int channel, uint8_t *arr, int buf_size) {
  int i;
  int temp_len = buf_size;
  uint8_t temp_buf[buf_size+1];
  const uint8_t start_sig = 170;
  while(temp_len > 0){
    temp_buf[temp_len] = arr[temp_len-1];
    temp_len--;
  }
  temp_buf[0] = start_sig;
  Serial.print("Writing");
  for(i=0;i<buf_size+1;i++){
    Serial.print(", ");
    Serial.print(temp_buf[i], DEC);
  }
  Serial.print(" to serial channel ");
  Serial.println(channel, DEC);
  switch(channel){
    case 1:
      mySerial_1.write(temp_buf, buf_size+1);
      break;
    case 2:
      mySerial_2.write(temp_buf, buf_size+1);
      break;
    case 3:
      mySerial_3.write(temp_buf, buf_size+1);
      break;
    case 4:
      mySerial_4.write(temp_buf, buf_size+1);
      break;
    default:
      Serial.println("Invalid channel selection for writebuffertoserial()");
      Serial.print("Error! cannot write to serial channel ");
      Serial.println(channel);
      break;
    }
}

void checkID(uint8_t ID){
  if(ID > 0 && ID <= 100){
    if(ID > IDs[0] || isFirst != 1){
      digitalWrite(LEDPIN, LOW);
      isFirst = 0;
    }else{
      digitalWrite(LEDPIN, HIGH);
    }
    if(!isvalueinarray(ID, IDs, id_count)){
      IDs[id_count] = ID;
      id_count++;
      Serial.print(ID, DEC);
      Serial.println(" added"); // to the ID list.");
      Serial.print("id_count "); //now equals: ");
      Serial.println(id_count, DEC);
      noNewID = 0;
    }else{
      noNewID++;
      if(noNewID > 50){
        Serial.print(id_count, DEC);
        Serial.println(" IDs found");
        collecting_IDs = 0;
      }
    }
  }
}

void setup(){
  Serial.begin(TERMBAUD);
  Serial.println("attempting to initialize Serial comms...");
  Serial.println("...");
  pinMode(RX_1, INPUT);
  pinMode(TX_1, OUTPUT);
  pinMode(RX_2, INPUT);
  pinMode(TX_2, OUTPUT);
  pinMode(RX_3, INPUT);
  pinMode(TX_3, OUTPUT);
  pinMode(RX_4, INPUT);
  pinMode(TX_4, OUTPUT);
  mySerial_1.begin(TRXRATE);
  Serial.println("...");
  //delay(500);
  mySerial_2.begin(TRXRATE);
  Serial.println("...");
  //mySerial_3.begin(TERMBAUD);
  //Serial.println("...");
  //mySerial_4.begin(TERNBAUD);
  pinMode(LEDPIN, OUTPUT);
  Serial.println("...initialization complete.");
  Serial.println("Generating random ID...");
  randomSeed(analogRead(A5));
  rand100 = random(1,101);
  IDs[0] = uint8_t(rand100);
  //IDs[1] = uint8_t(76);
  id_count = 1;
  collecting_IDs = 1;
  isFirst = 1;
  msg_buf[12] = 0;
  digitalWrite(LEDPIN, HIGH);
  prevTime = millis();
}

void loop(){
  if(collecting_IDs){
    //mySerial_1.write(msg_buf, 12);
    mySerial_1.listen();
    delay(1);
    writebuffertoserial(1, IDs, id_count);
    //writebuffertoserial(2, IDs, id_count);
    delay(7);
    Serial.print("The next value in the buffer for channel 1 is ");
    Serial.println(mySerial_1.peek());
    if(mySerial_1.read() == 170){
      while(mySerial_1.available() > 0){
        ID_read[0] = mySerial_1.read();
        //Serial.print(ID_read[0]);
        checkID(ID_read[0]);
      }
      //Serial.println("***");
    }
    mySerial_2.listen();
    delay(1);
    //writebuffertoserial(1, IDs, id_count);
    writebuffertoserial(2, IDs, id_count);
    delay(7);
    Serial.print("The next value in the buffer for channel 2 is ");
    Serial.println(mySerial_2.peek(), DEC);
    if(mySerial_2.read() == 170){
      while(mySerial_2.available() > 0){
        ID_read[1] = mySerial_2.read();
        checkID(ID_read[1]);
      }
      //Serial.println("***");
    }
  }
  if(isFirst == 1){
    blink_led(LEDPIN);
  }else{
    digitalWrite(LEDPIN, LOW);
  }
  //writebuffertoserial(1, IDs, id_count);
  //delay(7);
  //writebuffertoserial(2, IDs, id_count);
  delay(1);
}
